﻿using System.Collections;
using System.Collections.Generic;
using System.Linq;
using UnityEngine;


public class AStarWrapper
{
    private float Size;
    private int Width;
    private int Height;
    private bool[,] GoMap;

 
    private PathNode[,] Map;
    private List<PathNode> OpenList;
    private List<PathNode> CloseList;
    private List<PathNode> FoundPath = null;
    private List<PathNode> AfterFilteFoundPath = null;
    private PathNode StartGridPointer;
    private PathNode targetGridPointer;

    // 斜角移动花费
    public float DdCost = 14.14f;

    //直线移动花费
    public float DsCost = 10;


    /// <summary>
    /// A* 寻路出来的路径
    /// </summary>
    /// <returns></returns>
    public List<PathNode> GetFoundPath() {
        return this.FoundPath;
    }

    /// <summary>
    /// 优化A*寻路出来的路径之后的路径。让某些点直接走直线。
    /// </summary>
    /// <returns></returns>
    public List<PathNode> GetFilterFoundPath() {
        return this.AfterFilteFoundPath;
    }

    public Vector3[] GetFilterRoadData() {
        if (this.AfterFilteFoundPath == null) {
            return null;
        }

        Vector3[] ret = new Vector3[this.AfterFilteFoundPath.Count];
        for (int i = 0; i < this.AfterFilteFoundPath.Count; i++) {
            ret[i] = new Vector3(AfterFilteFoundPath[i].indexI * Size, 0, AfterFilteFoundPath[i].indexJ * Size);
        }

        return ret;
    }

    public Vector3[] GetRoadData()
    {
        if (this.FoundPath == null)
        {
            return null;
        }

        Vector3[] ret = new Vector3[this.FoundPath.Count];
        for (int i = 0; i < this.FoundPath.Count; i++)
        {
            ret[i] = new Vector3(FoundPath[i].indexI * Size, 0, FoundPath[i].indexJ * Size);
        }

        return ret;
    }


    //初始化
    public void InitWithMapData(bool[,] GoMap, int width, int height, float Size)
    {
        this.Size = Size;
        this.GoMap = GoMap;
        //创建开表，闭表
        OpenList = new List<PathNode>();
        CloseList = new List<PathNode>();
        AfterFilteFoundPath = new List<PathNode>();
        //StartGridPointer = null;
        //从Tile里面获取路径信息
        GreateMapFromTile(GoMap, width, height);
    }
    private void GreateMapFromTile(bool[,] GoMap, int width, int height)
    {
        Width = width;
        Height = height;
        Map = new PathNode[this.Width, this.Height];
        for (int i = 0; i < this.Height; i++) {
            for (int j = 0; j < this.Width; j++) {
                //读取信息创建
                Map[i, j] = CreateGridFromTile(i, j);
            }
        }
    }
    private PathNode CreateGridFromTile(int i, int j)
    {
        PathNode p = new PathNode();
        p.Traversable = GoMap[i, j];
        p.indexI = i;
        p.indexJ = j;
        return p;
    }


    //将世界空间的坐标信息转换到我这个图中的坐标
    private void WorldPositionTo(Vector3 WorldPosition, out int i, out int j)
    {
        i = Mathf.RoundToInt(WorldPosition.x / this.Size);
        j = Mathf.RoundToInt(WorldPosition.z / this.Size);
    }
    
    public bool AstarSearch(Vector3 startPosition, Vector3 endPosition) {
        int Si, Sj, Ei, Ej;
        WorldPositionTo(startPosition, out Si, out Sj);
        WorldPositionTo(endPosition, out Ei, out Ej);
        
        Debug.Log(Si + ":" + Sj);
        Debug.Log(Ei + ":" + Ej);

        StartGridPointer = Map[Si, Sj];
        targetGridPointer = Map[Ei, Ej];
        
        if (this.GoMap[Ei, Ej]){
            AstarMapPathFind();
            return true;
        }

        return false;
    }

    private void AstarMapPathFind()
    {
        //定义一个开表，其为需要评估的点
        //定义一个闭表，其为已经评估完了的点
        //将起始点加入到开表中
        this.ClearLastFindMemory();
        OpenList.Add(StartGridPointer);
        //循环，直到开表中没有节点
        while (OpenList.Count > 0)
        {
            // 选择一个 当前点 当前点是开表中需要评估的点中，评估出来最接近目标点的点
             PathNode currentNode = FindMinimalCostInOpenList();
            //将当前点从开表中移除并加入到闭表中
            OpenList.Remove(currentNode);
            CloseList.Add(currentNode);
            //如果我们当前点就是目标点，那么结束寻路
            if (currentNode == targetGridPointer)
            {
                RetracePath(StartGridPointer, targetGridPointer);
                return;
            }
            else
            {
                //查找当前点的所有邻接点
                PathNode[] Neiborgh = GetNeighbour(currentNode);
                for (int i = 0; i < Neiborgh.Length; i++)
                {
                    //如果某个邻接点已经在闭表中（即评估过）或者是不可通过的则跳过这个邻接点
                    if (CloseList.Contains(Neiborgh[i]) || !Neiborgh[i].Traversable)
                        continue;
                    float newGCost = GetDistance(currentNode, Neiborgh[i])+currentNode.Gcost;
                    //如果从当前点到到其邻接点更短或邻接点不在开表中
                    if (newGCost < Neiborgh[i].Gcost || !OpenList.Contains(Neiborgh[i]))
                    {
                        //那么计算邻接点的评估值
                        Neiborgh[i].Gcost = newGCost;
                        Neiborgh[i].Hcost = GetDistance(Neiborgh[i], targetGridPointer);
                        Neiborgh[i].TotalCost = Neiborgh[i].Gcost + Neiborgh[i].Hcost;
                        //将其上一节点设置成当前节点
                        Neiborgh[i].LastStepGrid = currentNode;
                        //如果邻接点不在开表中，就把他加如开表中
                        if (!OpenList.Contains(Neiborgh[i]))
                            OpenList.Add(Neiborgh[i]);
                    }
                }
            }
        }
        
    }

    /// <summary>
    /// 就是斜角 H = DdCost， 直走 H = DsCost
    /// </summary>
    /// <param name="curGrid"></param>
    /// <param name="tarGrid"></param>
    /// <returns></returns>
    private float GetDistance(PathNode curGrid, PathNode tarGrid)
    {
        //H对角 = min(X方向差值, Y方向差值)
        //斜角行走消耗
        float H_diagonal = Mathf.Min(Mathf.Abs(curGrid.indexI - tarGrid.indexI), Mathf.Abs(curGrid.indexJ - tarGrid.indexJ));
        //直线行走消耗
        //H直线  = X方向差值 + Y方向差值
        float H_straight = Mathf.Abs(curGrid.indexI - tarGrid.indexI) + Mathf.Abs(curGrid.indexJ - tarGrid.indexJ);
        //H(n) = Dd*H对角 + Ds*(H直线-2H对角)
        float HCost = DdCost * H_diagonal + DsCost * (H_straight - 2 * H_diagonal);
        return HCost;
    }
    private PathNode[] GetNeighbour(PathNode inGrid)
    {
        List<PathNode> Neighbours = new List<PathNode>();
        for (int x = -1; x <= 1; x++)
        {
            for (int y = -1; y <= 1; y++)
            {
                if (x == 0 && y == 0)
                    continue;

                int checkX = inGrid.indexI + x;
                int checkY = inGrid.indexJ + y;

                if (checkX >= 0 && checkX < Width && checkY >= 0 && checkY < Height)
                {
                    Neighbours.Add(Map[checkX, checkY]);
                }
            }
        }
        return Neighbours.ToArray();
    }
    private PathNode FindMinimalCostInOpenList()
    {
        PathNode path_Grid = OpenList[0];
        for (int i = 1; i < OpenList.Count; i++)
        {
            if (path_Grid.TotalCost >= OpenList[i].TotalCost)
            {
                path_Grid = OpenList[i];
            }
        }
        return path_Grid;
    }

    private void AddToCloseList(PathNode addingGrid)
    {
        CloseList.Add(addingGrid);
    }

    private bool IsInCloseList(PathNode inGrid)
    {
        return CloseList.Contains(inGrid);
    }

    private void RemoveFromOpenList(PathNode removingGrid)
    {
        OpenList.Remove(removingGrid);
    }

    private bool CheckPointAllPass(int i, int j)
    {
        if (i + 1 == Width || !GoMap[i + 1, j])
            return false;

        if (i == 0 || !GoMap[i - 1, j])
            return false;


        if (j + 1 == Height || !GoMap[i, j + 1])
            return false;

        if (j == 0 || !GoMap[i, j - 1])
            return false;


        if (i + 1 == Width || j + 1 == Height || !GoMap[i + 1, j + 1])
        {
            return false;
        }

        if (i == 0 || j == 0 || !GoMap[i - 1, j - 1])
        {
            return false;
        }

        if (i == 0 || j + 1 == Height || !GoMap[i - 1, j + 1])
        {
            return false;
        }

        if (i == Width || j == 0 || !GoMap[i + 1, j - 1])
        {
            return false;
        }

        return true;
    }

    private void FilterPath()
    {
        this.AfterFilteFoundPath.Clear();
        this.AfterFilteFoundPath.Add(FoundPath[0]);

        int i = 1;
        for (i = 1; i < FoundPath.Count - 1; i++) {
            if (CheckPointAllPass(FoundPath[i].indexI, FoundPath[i].indexJ)) {
                continue;
            }
            else {
                this.AfterFilteFoundPath.Add(FoundPath[i]);
            }
        }

        this.AfterFilteFoundPath.Add(FoundPath[i]);
    }

    private void RetracePath(PathNode startNode, PathNode endNode)
    {
        List<PathNode> path = new List<PathNode>();
        PathNode currentNode = endNode;

        while (currentNode != startNode)
        {
            path.Add(currentNode);
            currentNode = currentNode.LastStepGrid;
        }
        path.Add(StartGridPointer);
        path.Reverse();
        FoundPath = path;

        this.FilterPath();
    }
    
    //清除上一次寻路对当前网格的影响
    private void ClearLastFindMemory()
    {
        for (int j = 0; j < this.Height; j++)
        {
            for (int i = 0; i < this.Width; i++)
            {
                Map[i, j].Gcost = 0;
                Map[i, j].Hcost = 0;
                Map[i, j].TotalCost = 0;
                Map[i, j].LastStepGrid = null;
            }
        }
        OpenList.Clear();
        CloseList.Clear();
    }
}
